#pragma ident "$Id: SolverIFCBMixed.cpp 2877 2011-08-20 08:40:11Z yanweignss $"

/**
 * @file SolverIFCBMixed.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//
//  L5 = rou + ifcb;
//  gfif = LC15 - LC12 = -f5^2/(f1^2-f5^2)*ifcb
//
//============================================================================


#include "SolverIFCBMixed.hpp"
#include "SystemTime.hpp"

#define debug 0

namespace gpstk
{

      // Index initially assigned to this class
   int SolverIFCBMixed::classIndex = 9600000;

      // Returns an index identifying this object.
   int SolverIFCBMixed::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string SolverIFCBMixed::getClassName() const
   { return "SolverIFCBMixed"; }

   void SolverIFCBMixed::Init(void)
   {
       // set up variables and equations for PPP

       //////////////////////////////////////////////////////////////
       //
       // GFIF(1,2,5) = IF(1,2) - IF(1,5) = ifcb + const;
       //
       //////////////////////////////////////////////////////////////
       bg15 = ( 1.0           /( GAMMA_GPS_L1L5 - 1.0 ) );

   }

   void SolverIFCBMixed::setUpEquations(void)
   {
   } // end of setUpEquations



   sourceTypeValueMap& SolverIFCBMixed::Process(CommonTime& epoch,
                                             const SatID& sat, 
                                             sourceTypeValueMap& srcTypeValueData)
   {
       if(firstTime)
       {
            // srcArcData setting
            for(sourceTypeValueMap::iterator stvIt=srcTypeValueData.begin();
                    stvIt!=srcTypeValueData.end();
                    stvIt++)
            {
                SourceID source( (*stvIt).first );
                typeValueMap tvData ( (*stvIt).second );

                // set intial value
                if(srcArcData.find(source) == srcArcData.end())
                {
                    srcArcData[source] = 0.0;
                }

                // reset the arc
                double arcNum = tvData(TypeID::satArc);
                if(srcArcData[source] != arcNum)
                {
                    srcArcData[source] = arcNum;
                }
            }

            ifcbDiff = 0.0;
            ifcbUndiff = 0.0;
            epochDiffIFCBData[epoch] = 0.0;
            epochUndiffIFCBData[epoch] = 0.0;

            // store the old data;
            lastSrcData.clear();
            lastSrcData = srcTypeValueData;

            // no longer first time
            firstTime = false;

            return srcTypeValueData;
       }
       else
       {
            std::map<SourceID,double> diffGFIFData;
            sourceCSData.clear();
            sourceGFIFData.clear();
            for(sourceTypeValueMap::iterator stvIt=srcTypeValueData.begin();
                    stvIt!=srcTypeValueData.end();
                    stvIt++)
            {
                SourceID source( (*stvIt).first );
                typeValueMap tvData ( (*stvIt).second );
                sourceGFIFData[source] = tvData(TypeID::GFIF);

                // set intial value
                if(srcArcData.find(source) == srcArcData.end())
                {
                    srcArcData[source] = 0.0;
                }

                bool csOcurred(false);
                double arcNum = tvData(TypeID::satArc);
                if(srcArcData[source] != arcNum)
                {
                    csOcurred = true;
                    srcArcData[source] = arcNum;
                    sourceCSData[source] = true;
                }

                // diffGFIFData
                if(lastSrcData.find( source )!=lastSrcData.end())
                {
                    if(!csOcurred)
                    {
                        typeValueMap lastTvData = lastSrcData[source];
                        double lastGFIF = lastTvData(TypeID::GFIF);
                        double currGFIF = tvData(TypeID::GFIF);
                        double diff = currGFIF - lastGFIF;
                        diffGFIFData[source] = diff;
                    }
                }
            };

            // mean diff
            if(diffGFIFData.size() == 0 )
            {
                cerr << "SolverIFCBMixed:: data break ocurred, data not enough:" << endl;
                exit(-1);
            }

            // compute the mean diffGFIF
            double meanDiff(0.0);
            for(std::map<SourceID,double>::iterator srcIt=diffGFIFData.begin();
                    srcIt!=diffGFIFData.end();
                    srcIt++)
            {
                meanDiff += (*srcIt).second;
            }

            meanDiff = meanDiff/double(diffGFIFData.size());

            // IFCB = IFCB + meanDiff;
            ifcbDiff = ifcbDiff + meanDiff;
            epochDiffIFCBData[epoch] = ifcbDiff;

            // now, let's compute the ambiguities by deducting the ifcb
            for(std::map<SourceID, double>::iterator srcIt=sourceGFIFData.begin();
                    srcIt!=sourceGFIFData.end();
                    srcIt++)
            {
                SourceID source = (*srcIt).first;
                double amb = (*srcIt).second - ifcbDiff; 
                bool cs = sourceCSData[source];
                // smooth the ambiguities for current source
                smoothAmb(source, cs, amb);
            }

            // at last, compute the ifcb again with updated smoothed amb
            std::map<SourceID, double> ifcbData;

            ifcbUndiff = 0.0;
            for(std::map<SourceID,double>::iterator srcIt=sourceGFIFData.begin();
                    srcIt!=sourceGFIFData.end();
                    srcIt++)
            {
                ifcbUndiff+= (*srcIt).second - ambData[(*srcIt).first].meanAmb;
            }

            ifcbUndiff = ifcbUndiff/double(sourceGFIFData.size());
            epochUndiffIFCBData[epoch] = ifcbUndiff;

            // store data for next-time computation
            lastSrcData.clear();
            // store the old data;
            lastSrcData = srcTypeValueData;

            return srcTypeValueData;
       }
   };


    double SolverIFCBMixed::smoothAmb( const SourceID& source,
                                    bool csOcurred,
                                    const double& amb )
    {

        // Increment window size
        ++ambData[source].windowSize;

        /**
         *  here we don't consider the epoch flag from RINEX,
         *  which means we don't treat the epoch flag as true.
         */
        if( csOcurred )
        {
            // We reset the filter with this
            ambData[source].windowSize = 1;
        }

        // Let's prepare for the next time
        // If a cycle-slip happened or just starting up
        if(ambData[source].windowSize < 2)
        {
            ambData[source].meanAmb = amb;
            //give varAmb=1.0 as init
            ambData[source].varAmb = 0.02*0.02;
        }
        else
        {
            // Amb bias from the mean value
            double ambBias(amb - ambData[source].meanAmb);
            double size( static_cast<double>(ambData[source].windowSize) );

            // Compute average
            ambData[source].meanAmb += ambBias / size;

            // Compute variance
            // Var(i) = Var(i-1) + [ ( amb(i) - meanAmb)^2/(i)- 1*Var(i-1) ]/(i);
            ambData[source].varAmb  += ( ambBias*ambBias - ambData[source].varAmb ) / size;
        }

        return 0;


    }  // End of method 'AmbCSDetector::getDetection()'



   // print float solutions
   void SolverIFCBMixed::printOut(string file)
   {
       ifcbStream.open(file);
       for(std::map<CommonTime, double>::iterator it=epochDiffIFCBData.begin();
           it!=epochDiffIFCBData.end();
           it++)
       {
           YDSTime time((*it).first);
           double ifcb = (*it).second;

           ifcbStream << time.year << " "
                      << time.doy  << " "
                      << time.sod  << " ";
           ifcbStream << ifcb << " " ;
           ifcbStream << endl;
       }
   }

}  // End of namespace gpstk
